#include "path_generate/p2p/jps.h"
#include "core/utils/graphis_utils.h"
#include <queue>
#include <unordered_map>
#include <vector>
#include <algorithm>  // for std::reverse

struct CompareNodes {
    bool operator()(const GridNodePtr& lhs, const GridNodePtr& rhs) const {
        return lhs->getF() > rhs->getF() 
            || (lhs->getF() == rhs->getF() && lhs->getH() > rhs->getH());
    }
};


JPSPlanner::JPSPlanner(){ 
    debugMapPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/grid_expend_map", 1, true);
    jumpPointPub_ = nh_.advertise<visualization_msgs::MarkerArray>("/jps/jump_points", 1, true);
    debug_ = true;
}


// 路径规划
void JPSPlanner::genGridPath(int sx, int sy, int gx, int gy, vector<pair<int, int>>& points){
    LOG(INFO) <<  "start: (" << sx << "," << sy
        << "), goal: (" << gx << "," << gy << ")";

    if(debug_){ //调试模式
        expend_points_.clear();
        jump_points_.clear();
        debugGrid_ = getOccupancyGrid();
    }
    int width = getWidth();
    int height = getHeight();

    // 设置起点、终点
    nodeStart_ = GridNode(sx, sy, 0, 0);
    nodeStart_.setIdx(width);

    nodeGoal_ = GridNode(gx, gy, 0, 0);
    nodeGoal_.setIdx(width);

    // 初始化列表
    nodeStart_.updateH(nodeGoal_);
    std::priority_queue<GridNodePtr, std::vector<GridNodePtr>, CompareNodes> openList;
    std::unordered_map<int, float> closedList;  //存储G值
    openList.push(std::make_shared<GridNode>(nodeStart_));
    
    int iteration = 0;
    while(!ros::isShuttingDown() && !openList.empty()){
        iteration++;
        if(debug_){ //调试模式
            cout << "iteration: " << iteration << endl;
            if(iteration > 10){
                break;
            }
        }

        // 获取优先级最高节点
        auto nodePred = openList.top();
        openList.pop();
        int idxPred = nodePred->getIdx();

        if(closedList.find(idxPred) != closedList.end()){
            continue;
        }
        closedList[idxPred] = nodePred->getG();

        if(debug_){ //调试模式
            expend_points_.insert(idxPred);
            // cout << "idxPred: " << idxPred 
            //     << ", x: " << nodePred->getX() 
            //     << ", y: " << nodePred->getY()
            //     << ", cost: " << nodePred->getF()
            //     << endl;
        }

        // 发现目标节点
        if(*nodePred == nodeGoal_){
            LOG(INFO) << "find goal. iteration: " << iteration;
            vector<pair<int, int>> origin_points;
            while(nodePred->getPred() != nullptr){
                origin_points.emplace_back(make_pair(nodePred->getX(), nodePred->getY()));
                nodePred = nodePred->getPred();
            } 
            // 添加起点
            origin_points.emplace_back(make_pair(nodePred->getX(), nodePred->getY()));
            std::reverse(origin_points.begin(), origin_points.end());   
            LOG(INFO) << "origin_points size: " << origin_points.size();

            // // 裁剪路径
            // vector<pair<int, int>> pruning_points;
            // PathPruning(origin_points, pruning_points);
            // LOG(INFO) << "pruning points size: " << points.size();

            // // 填充直线
            // if(!pruning_points.empty()){
            //     for (size_t i = 0; i < pruning_points.size()-1; ++i){
            //         vector<pair<int, int>> line_points;
            //         utils::graphis::BresenhamLine(pruning_points[i].first, pruning_points[i].second,
            //             pruning_points[i+1].first, pruning_points[i+1].second, line_points);
            //         if(!line_points.empty()){
            //             points.insert(points.end(), line_points.begin(), line_points.end() - 1);
            //         }
            //     }
            //     points.emplace_back(pruning_points.back());
            // }

            points = origin_points;
            break;
        }

        // 遍历邻域节点
        for(int i = 0; i < GridNode::dir; i++) {
            auto nodeSucc = JumpPoint(nodePred, GridNode::dx[i], GridNode::dy[i]);
            if(nodeSucc == nullptr) {
                continue;
            }
            // nodeSucc->setPred(nodePred);
            int idxSucc = nodeSucc->getIdx();
            
            if(debug_){
                int idx = (height - nodeSucc->getY()) * width + nodeSucc->getX();
                if(idx >= 0 && idx < debugGrid_.data.size())
                    debugGrid_.data[idx] = max(50, int(debugGrid_.data[idx]));
            }

            // 障碍物检测 是否在闭列表
            if(closedList.find(idxSucc) != closedList.end()){
                continue;
            }
            // LOG(INFO) << "--> i: " << i 
            //     << ", idxSucc: " << idxSucc 
            //     << ", x: " << nodeSucc->getX() 
            //     << ", y: " << nodeSucc->getY()
            //     << ", g: " << nodeSucc->getG()
            //     << ", h: " << nodeSucc->getH()
            //     << ", cost: " << nodeSucc->getF();
            openList.push(nodeSucc);   

        }
    }

    if(debug_){ //调试模式
        LOG(INFO) << "openList size: " << openList.size() 
            << ", closedList size: " << closedList.size();
        for(auto idx : expend_points_){
            if(idx >=0 && idx < debugGrid_.data.size()){
                debugGrid_.data[idx] = 30;
            }
        }
        debugMapPub_.publish(debugGrid_);

        // 发布跳点数据
        vector<geometry_msgs::Point> marker_points;
        for(auto idx : jump_points_){
            int mx,my;
            indexToMap(idx, mx, my);
            geometry_msgs::Point point;
            mapToWorld(mx, my, point.x, point.y);
            marker_points.emplace_back(point);
        }
        visualization_msgs::MarkerArray markerArray_msg;
        visualization_msgs::Marker marker_msg
            = rvizMarker_.genSphereListMarker(marker_points, 0.05);
        rvizMarker_.setMarkerHeader(marker_msg, "map", "jps");
        rvizMarker_.setMarkerId(marker_msg, 0);
        rvizMarker_.setMarkerColor(marker_msg, rvizMarker_.colorBlue(0.5));
        markerArray_msg.markers.push_back(marker_msg);
        jumpPointPub_.publish(markerArray_msg);
    }
}


// 检测跳点
GridNodePtr JPSPlanner::JumpPoint(GridNodePtr node, int dir_x, int dir_y){
    int x = node->getX();
    int y = node->getY();

    int width = getWidth();
    int height = getHeight();

    int newX = x + dir_x;
    int newY = y + dir_y;
    auto nodeSucc = std::make_shared<GridNode>(newX, newY, node->getG(), 0); 
    // nodeSucc->setPred(node);
    nodeSucc->setIdx(width); 
    nodeSucc->updateG();
    nodeSucc->updateH(nodeGoal_);

    if(debug_){
        int idx = nodeSucc->getIdx();
        if(idx >= 0 && idx < debugGrid_.data.size())
            debugGrid_.data[idx] = max(10, int(debugGrid_.data[idx]));
        jump_points_.insert(idx);
    }

    // cout << "[JumpPoint 002] --> x: " << x << ", y: " << y << ", idx: " << nodeSucc->getIdx() << std::endl;

    // 障碍物检测
    if(!valueIsValid(x, y)){
        return nullptr;
    }

    // 终点检测
    if(*nodeSucc == nodeGoal_){
        return nodeSucc;
    }

    if(dir_x && dir_y){     // 对角线检测
    // cout << "[JumpPoint 003] ......" << std::endl;
        // horizontal 水平分量检测
        // vertical 垂直分量检测
        auto horizontal = JumpPoint(nodeSucc, dir_x, 0);
        auto vertical = JumpPoint(nodeSucc, 0, dir_y);
        if(horizontal || vertical){
            return horizontal ? horizontal : vertical;
        }
        // if(JumpPoint(nodeSucc, dir_x, 0) || JumpPoint(nodeSucc, 0, dir_y)){
        //     return nodeSucc;
        // }
    }
    else{   // 强制邻居检测
        // cout << "[JumpPoint 004] ......" << std::endl;
        if(detectForceNeighbor(nodeSucc, dir_x, dir_y)){
            return nodeSucc;
        }
    }

    // cout << "[JumpPoint 005] ......" << std::endl;
    return JumpPoint(nodeSucc, dir_x, dir_y);    
}


// 检测强制邻居
bool JPSPlanner::detectForceNeighbor(GridNodePtr node, int dir_x, int dir_y){
    int x = node->getX();
    int y = node->getY();

    // 水平检测 horizontal
    if(dir_x && !dir_y){
        if( !valueIsValid(x, y+1) && 
             valueIsValid(x + dir_x, y+1)){
            return true;
        }

        if( !valueIsValid(x, y-1) && 
             valueIsValid(x + dir_x, y-1)){
            return true;
        }
    }

    // 垂直检测 vertical
    if(!dir_x && dir_y){
        if( !valueIsValid(x+1, y) && 
             valueIsValid(x + 1, y + dir_y)){
            return true;
        }

        if( !valueIsValid(x-1, y) && 
             valueIsValid(x - 1, y + dir_y)){
            return true;
        }
    }

    // 斜对角检测 diagonal
    if(dir_x && dir_y){
        if( !valueIsValid(x - dir_x, y) && 
             valueIsValid(x - dir_x, y + dir_y)){
            return true;
        }

        if( !valueIsValid(x, y - dir_y) && 
             valueIsValid(x + dir_x, y - dir_y)){
            return true;
        }
    }

    return false;
}

// 路径裁剪
void JPSPlanner::PathPruning(vector<pair<int, int>>& points_in, vector<pair<int, int>>& out_points){
    if(points_in.empty()) return;
    vector<pair<int, int>> points = points_in;
    out_points.clear();
    size_t n = points.size();
    // 添加起始点
    out_points.push_back(points[0]);

    size_t i = 0; // 当前点索引
    while (i < n - 1) {
        bool pruned = false; // 是否找到可直接跳过的点

        // 从当前点尝试连接更远的点
        for (size_t j = n - 1; j > i; --j) {
            vector<pair<int, int>> line_points;
            utils::graphis::BresenhamLine(points[i].first, points[i].second,
                    points[j].first, points[j].second, line_points);

            // 检查直线上的每个点是否有障碍物
            bool has_obstacle = false;
            for (auto& point : line_points) {
                if (!valueIsValid(point.first, point.second)) {
                    has_obstacle = true;
                    break;
                }
            }

            if (!has_obstacle) {
                // 如果无障碍，则从 i 直接跳到 j
                out_points.push_back(points[j]);
                i = j; // 更新当前点索引
                pruned = true;
                break;
            }
        }

        // 如果无法跳过，则只能移动到下一个点
        if (!pruned) {
            ++i;
            if (i < n - 1) {
                out_points.push_back(points[i]);
            }
        }
    }

    // 添加终点
    out_points.push_back(points.back());
}
